#pragma config(Sensor, S1,     HTANG_LEFT,     sensorI2CCustom)
#pragma config(Sensor, S4,     HTANG_RIGHT,    sensorI2CCustom)
#pragma config(Sensor, S2,     TOUCH_SENSOR,    sensorTouch)
#pragma config(Sensor, S3,     SONAR,          sensorSONAR)
#pragma config(Motor,  motorA,          MOTOR_LEFT,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          MOTOR_RIGHT,   tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          MOTOR_PUSH,   tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#pragma platform(NXT)

#ifndef _ENCODER_EXTERN
#define _ENCODER_EXTERN // prevent multiple compilations

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: ROBOTC PROJECT MAIN FILE DECLARATION
#ifndef PROJECT_MAIN_FILE
#define PROJECT_MAIN_FILE "cencoderExtern.c" //!< enable compilation of task main() and various debug and test functions of this file
#endif
// END OF:  ROBOTC PROJECT MAIN FILE DECLARATION
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: HEADERS
#include "cencoderExtern.h"
// END OF:  HEADERS
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: GLOBAL INSTANCES
//ratio vTops/ticks for left extern encoder
long leftExternEncoderRatio = 803;
//ratio vTops/ticks for right extern encoder
long rightExternEncoderRatio = 803;
// END OF:  GLOBAL INSTANCES
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: EXTERNAL SOURCES
#ifndef _NXT
#include "cnxt.c"
#endif
// END OF:  EXTERNAL SOURCES
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: LOCAL INSTANCES
static long lastExtLeft = 0;
static long lastExtRight = 0;
// END OF:  LOCAL INSTANCES
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: CODES
void externEncoder_Init(void)
{
	lastExtLeft = 0;
	lastExtRight = 0;

	//reset des encoders
	nxt_ResetAngleEncoder();
}

/*
void externEncoder_SetDist(float dist)
{
	distEncoderMeter = dist;
	distEncoder = dist / valueVTops;
	writeDebugStreamLine("distEncoderMeter:%f  distEncoder:%f", distEncoderMeter, distEncoder);
}

void externEncoder_SetResolution(long leftTicksPerM, long rightTicksPerM)
{
	if(leftTicksPerM > rightTicksPerM)
	{
		leftExternEncoderRatio = VTOPS_PER_TICKS;
		rightExternEncoderRatio = VTOPS_PER_TICKS * rightTicksPerM/(float)leftTicksPerM;
		valueVTops = 1/(float)(VTOPS_PER_TICKS * leftTicksPerM);
		writeDebugStreamLine("leftTicksPerM > rightTicksPerM : valueVTops:%f", valueVTops);
	}
	else
	{
		leftExternEncoderRatio = VTOPS_PER_TICKS * leftTicksPerM/(float)rightTicksPerM;
		rightExternEncoderRatio = VTOPS_PER_TICKS;
		valueVTops = 1/(float)(VTOPS_PER_TICKS * rightTicksPerM);
		writeDebugStreamLine("leftTicksPerM <= rightTicksPerM : valueVTops:%f", valueVTops);
	}

	//recompute values involving vTops
	externEncoder_SetDist(distEncoderMeter);
}
*/
bool externEncoder_ReadSensor(long *dLeft, long *dRight, long *dAlpha, long *dDelta)
{
	//static float dTheta;
	//static float dV;
	static long left, right;

	//read encoder
	//left = -lm629_ReadRealPosition( LWHEEL );
	//right = lm629_ReadRealPosition( RWHEEL );
	left = nxt_ReadAngleEncoderLeft();
	right = nxt_ReadAngleEncoderRight();

	//convert units and save position
	left *= leftExternEncoderRatio;
	right *= rightExternEncoderRatio;

	//compute delta for left wheel
	*dLeft = left - lastExtLeft;
	lastExtLeft = left;
	//verify left encoder overflow
	//CORRECT_DELTA_OVERFLOW(*dLeft, MAX_EXTERN_ENCODER_ABS_VTOPS);

	//compute delta for right wheel
	*dRight = right - lastExtRight;
	lastExtRight = right;
	//verify right encoder overflow
	//CORRECT_DELTA_OVERFLOW(*dRight, MAX_EXTERN_ENCODER_ABS_VTOPS);

	//compute alpha and delta displacement
	*dAlpha = (*dRight-*dLeft)/2;
	*dDelta = (*dRight+*dLeft)/2;

	return true;
}

// END OF:  CODES
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>


//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// SECTION: TESTS

#if(PROJECT_MAIN_FILE == "cencoderExtern.c")
	//-----------------------------------------------------------------------------------------------------------------------------
	//! @brief     this task tests the functions
	//! @return    none
	//! @author    cho
	//! @note
	//! @todo
	//-----------------------------------------------------------------------------------------------------------------------------
	task main()
	{
		static long dLeft = 0, dRight = 0;
		static long dAlpha = 0, dDelta = 0;

		static long dLeft2 = 0, dRight2 = 0;
		static long dAlpha2 = 0, dDelta2 = 0;

		//reset encoder
		encoder_Init();
		externEncoder_Init();
		//encoder_SetDist(0.1075); //meter

		//configure encoder
		//DIST_PER_TICK = 1.0*WHEEL_DIAMETER80*PI/360.0 //mm
		encoder_SetResolution(1446, 1446); //tickPerMeter
		nxt_ResetAngleEncoder();
		//motor[MOTOR_LEFT] = -10;
		//motor[MOTOR_RIGHT] = -10;
		nxt_ApplyMotorLeft(10);
		nxt_ApplyMotorRight(10);
		//Sleep(5000);
		while(1)
		{
			externEncoder_ReadSensor(&dLeft,&dRight,&dAlpha,&dDelta);
			encoder_ReadSensor(&dLeft2,&dRight2,&dAlpha2,&dDelta2);
			writeDebugStreamLine("");
			writeDebugStreamLine("dLeft externe:%d dLeft2 interne:%d", dLeft, dLeft2);
			writeDebugStreamLine("dRight externe:%d dRight2 interne:%d", dRight, dRight2);
			Sleep(500);
		}
	}
#endif // (PROJECT_MAIN_FILE)


// END OF:  TESTS
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

#endif
